#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

namespace enc = sensor_msgs::image_encodings;

//Global variables to be pushed by saveImage()
/*
cv_bridge::CvImagePtr cv_ptr_left;
cv_bridge::CvImagePtr cv_ptr_right;
cv_bridge::CvImagePtr cv_ptr_disparity;

//cv_bridge::CvImage cv_left;
//cv_bridge::CvImage cv_right;
//cv_bridge::CvImage cv_disparity;
sensor_msgs::Image left;
sensor_msgs::Image right;
sensor_msgs::Image disparity;
*/
class Saver
{

	Saver()
	{
		ROS_INFO("Constructor");
		ros::NodeHandle nh;

		image_transport::ImageTransport it(nh);

		image_transport::Subscriber sub = it.subscribe("/stereo/left/image_raw", 5, imageLeftCallback);
		image_transport::Subscriber sub2 = it.subscribe("/stereo/right/image_raw", 5, imageRightCallback);
		image_transport::Subscriber sub3 = it.subscribe("/stereo/disparity", 5 , disparityCallback);
		
	}

	public:
		cv_bridge::CvImagePtr cv_ptr_left;
		cv_bridge::CvImagePtr cv_ptr_right;
		cv_bridge::CvImagePtr cv_ptr_disparity;

	void imageLeftCallback(const sensor_msgs::ImageConstPtr& original_image)
	{

		try
		{
			cv_ptr_left = cv_bridge::toCvCopy(original_image, enc::BGR8);
		}
		catch (cv_bridge::Exception& e)
		{

			ROS_ERROR("main.cpp::cv_bridge exception: %s", e.what());
			return;
		}
	

	}

}

void imageLeftCallback(const sensor_msgs::ImageConstPtr& original_image)
{
	
}


void imageRightCallback(const sensor_msgs::ImageConstPtr& original_image)
{
right=*original_image;
}

void disparityCallback(const sensor_msgs::ImageConstPtr& original_image)
{
disparity=*original_image;
}

void saveImages(void){
	

	cv::imwrite("/tftpboot/left.png",cv_ptr_left->image);
	cv::imwrite("/tftpboot/right.png",cv_ptr_right->image);
	cv::imwrite("/tftpboot/disparity.png",cv_ptr_disparity->image);

}


int main(int argc, char **argv)
{
        ros::init(argc, argv, "imageSaver");
        ros::NodeHandle nh;

        image_transport::ImageTransport it(nh);

        image_transport::Subscriber sub1 = it.subscribe("/stereo/left/image_raw", 5, imageLeftCallback);
        image_transport::Subscriber sub2 = it.subscribe("/stereo/right/image_raw", 5, imageRightCallback);
	image_transport::Subscriber sub3 = it.subscribe("/stereo/disparity", 5 , disparityCallback);

	ros::Rate r(1);
	while(ros::ok())
	{
		saveImages();
		ros::spinOnce();
		r.sleep();
	}

}
